#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include "base_interface_demo01/action/progress.hpp"
using base_interface_demo01::action::Progress;
using namespace std::placeholders;
class ProgressActionServerNode: public rclcpp::Node
{
private:
    rclcpp_action::Server<Progress>::SharedPtr server_;
public:
    ProgressActionServerNode():Node("ProgressActionServerNode")
    {
        RCLCPP_INFO(this->get_logger(),"创建progress_action_server_node_cpp");
        this->server_=rclcpp_action::create_server<Progress>(
             this,
            "get_sum",
            std::bind(&ProgressActionServerNode::handle_goal, this, _1, _2),
            std::bind(&ProgressActionServerNode::handle_cancel, this, _1),
            std::bind(&ProgressActionServerNode::handle_progress_resp, this, _1)  
        );
    }    
private:
     
    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, 
        std::shared_ptr<const Progress::Goal> goal)
    {
        (void)uuid; //没有使用的参数最好这样写 避免编译器警告 这个是为客户端生成的唯一标识
        if(goal->num<=1)
        {
            RCLCPP_INFO(this->get_logger(),"提交的goal num 必须大于1");
            return rclcpp_action::GoalResponse::REJECT;
        }
        RCLCPP_INFO(this->get_logger(),"提交的goal num=%d合法",goal->num);
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
     
    rclcpp_action::CancelResponse handle_cancel(
        std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> cancel_handle)
    {
        (void)cancel_handle;
        RCLCPP_INFO(this->get_logger(),"接收到处理请求，取消完成");
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    void execute(std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
        int num=goal_handle->get_goal()->num;
        int sum = 0;
        std::shared_ptr<Progress::Result> result = std::make_shared<Progress::Result>();
        // Progress::Feedback 等价于
        //base_interface_demo01::action::Progress_Feedback
        auto feedback=std::make_shared<Progress::Feedback>();
        rclcpp::Rate rate(1.0);//1hz 1秒一次  2 一秒2次 赫兹（Hz）​，表示“每秒多少次”
        for(int i=1;i<=num;i++)
        {
            sum+=i;
            double progress=i/(double)num;
            
            feedback->progress=progress;
            goal_handle->publish_feedback(feedback);
            RCLCPP_INFO(this->get_logger(),"连续反馈中，进度%.2f",progress);
            //处理取消请求 如ctrl+c
            if(goal_handle->is_canceling())
            {
                result->sum=sum;
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(),"任务被取消了");
                return;
            }
            rate.sleep();
        }
        if(rclcpp::ok())
        {
            result->set__sum(sum);
            RCLCPP_INFO(this->get_logger(),"最终响应结果%d",sum);
            goal_handle->succeed(result);
        }
    }
    // 生成连续的反馈和最终响应
    void handle_progress_resp(
        std::shared_ptr<rclcpp_action::ServerGoalHandle<Progress>> goal_handle)
    {
        std::thread(std::bind(&ProgressActionServerNode::execute,this,goal_handle)).detach();
    }

};
int main(int argc, char ** argv)
{
    rclcpp::init(argc,argv);
    rclcpp::spin(std::make_shared<ProgressActionServerNode>());
    rclcpp::shutdown();
    return 0;
}